#include "Trajectory.h"
#include <stdio.h>
#include <iostream>

#include "transitHub.h"
#include <unistd.h>
using namespace std;
Trajectory::Trajectory()
{
    //ctor
    dt=0.02;H=26;
}

Trajectory::~Trajectory()
{
    //dtor
}

void Trajectory::push_back(Vector3f pos)
{
    m_points.push_back(pos);
    int s=m_points.size();
    if(s>1)
    {
        m_velocity.push_back( pos-m_points[s-2]);
    }
    if(s>2)
    {
        m_accel.push_back(m_velocity[s-2]-m_velocity[s-3]);
    }
}
Vector3f Trajectory::calZMP()
{
    int s=m_points.size();
    if(s<3)
        return Vector3f(0,0,0);

    double k=dt*dt*980/H;//unit cm ,g=980
    Vector3f z;
    z=(-m_points[s-3]+(2+k)*m_points[s-2]-m_points[s-1])/k;
    return z;

}

 InvertPendulum::InvertPendulum(double x0,double d_cm,double h_c,double dt)
{
    m_x0=x0;m_d_cm=d_cm;m_h_C=h_c;m_dt=dt;
    int data[20]={0};
    m_rob.modifyData(data);
}

void InvertPendulum::speed_up(double t)
{
    static double dt=0.01;
 static double k=sqrt(980/m_h_C);
    double d,rx;
    double y=m_x0;
    double v=m_x0*k;
        d=y+v*t;
        rx=atan((-d+3.5)/m_h_C);
        double len=m_h_C/cos(rx);
        m_rob.setLeg(rx,0,0,m_h_C/cos(rx),1);
        m_rob.setLeg(rx,0,0,m_h_C/cos(rx),2);
           int data[20];
        m_rob.getData(data);
        transitHub::getInstance()->doLoopTx(data);
    cout<<m_rob.cal_referCentroid(1)(1)<<endl;
    getchar();
    getchar();
    while(t>=0)
    {
        t=t-dt;
        d=y+v*t;
        rx=atan((-d+3.5)/m_h_C);
        double len=m_h_C/cos(rx);
        m_rob.setLeg(rx,0,0,m_h_C/cos(rx),1);
        m_rob.setLeg(rx,0,0,m_h_C/cos(rx),2);
        int data[20];
        m_rob.getData(data);
        transitHub::getInstance()->doLoopTx(data);
        usleep(10*1000);
        //cout<<m_rob.calHip(2)(1)<<endl;
    }


}
double InvertPendulum::swing_left(double dy)//
{
    static double k=sqrt(980/m_h_C);
    double y=m_x0;
    double v=-y*k;
    double tem=0;
    double t=0;
    while(t>=0)
    {
        double x=y*cosh(k*(t))+v*sinh((t)*k)/k+m_d_cm;
        double rx=atan((-x+3.5)/m_h_C);
        double len=m_h_C/cos(rx);
        m_rob.setLeg(rx,0,0,m_h_C/cos(rx),2);
        m_rob.setLeg(rx,0,0,m_h_C/cos(rx)-(t)*5,1);
        t+=0.01;

        int data[20];
        m_rob.getData(data);
        transitHub::getInstance()->doLoopTx(data);
        double c=m_rob.cal_referCentroid(2)(1);
        cout<<t<<","<<c<<endl;
        if(abs(c-tem)<dy)
            break;
        usleep(10*1000);
        tem=c;
    }

    return 0;
}
